#include <robotLac.h>
#include <Servo.h>
#include <Tableros.h>

//-Sensores y Hardware------------
/**/robotLac robot(28);
/**/Infrared_Sensor infI(A1), infD(A0);
/**/Color_Sensor ColorSensor(27, 25, 24, 26,22);
/**/int sensorI, sensorD;
/**/Distance_Sensor dSensor(31,29);
//---------------------------------

/**/char INBYTE; //Entrada del bluetooth
const float DISTANCIAPARED=0.06;


Tablero tab("rwy");
void BuscarColor(Tablero tab, int destino);
void setup()
{
  Serial3.begin(9600);
  pinMode(23,OUTPUT);
  ColorSensor.begin();
  digitalWrite(23, LOW);
  robot.begin();
  robot.detener();
  dSensor.begin();
  Serial3.print("El tablero tiene");Serial3.print(tab.cantColores); Serial3.println("Colores");
  
}

void loop()
{
  
  if(Serial3.available()>0){
    INBYTE = Serial3.read();
    Serial3.print("Recibi: ");
    Serial3.println(INBYTE);
    BuscarColor(tab,INBYTE-48);
    BuscarLinea();
    SeguirLinea();
    Serial3.println("Volvi al main porq soy un hijo de puta");
  }
  delay(1000);
}



void BuscarColor(Tablero tab,int destino){
  ColorSensor.calc_RGB();
  int origen = tab.definirOrigen(ColorSensor.get_R(), ColorSensor.get_G(), ColorSensor.get_B());
  while(destino!=origen){
    if(sensorI >= 500 && sensorD >= 500){}
    else if(origen==-1){origen = tab.definirOrigen(ColorSensor.get_R(), ColorSensor.get_G(), ColorSensor.get_B());}
    else if(destino>origen){robot.avanzar();}
    else if(destino<origen){robot.retroceder();}
    else {break;}
    ColorSensor.calc_RGB(); 
    origen = tab.definirOrigen(ColorSensor.get_R(), ColorSensor.get_G(), ColorSensor.get_B());
     delay(25);
   }
  robot.detener();
}

void BuscarLinea(){
  while(infD.read() < 500 && infI.read() < 500){
          robot.avanzar();
 }
 robot.detener();
 delay(2000);
 while(infD.read() >= 500 && infI.read() >= 500){
          robot.avanzar();
 }
 robot.detener();
 delay(2000);
 robot.avanzar();
 delay(500);
 while(infD.read() < 500 && infI.read() < 500){
          robot.horario();
 }
 robot.detener();
}
void SeguirLinea(){
 robot.detener();
 boolean adelante=true;
 boolean volviendo=false;
 while(true){
   sensorI=infI.read();
   sensorD=infD.read();
   if(dSensor.get_distance()<DISTANCIAPARED){
       if(volviendo){robot.detener(); robot.horario(); delay(1500); robot.avanzar(); delay(750);robot.detener(); return;}
       adelante=false;
      } 
 if(adelante){
      
      if(sensorI >= 500 && sensorD >= 500){
        Serial3.println("Entre 1");
        robot.avanzar();
        }
      else if(sensorI >= 500 && sensorD < 500){
        Serial3.println("Entre 2");
        robot.antihorario();
        }
      else if(sensorI < 500 && sensorD >= 500){
        Serial3.println("Entre 3");
        robot.horario();
        }
      else{
        Serial3.println("Entre 4");
        int tiempoini=millis();
          while(infD.read() < 500 && infI.read() < 500){
           Serial3.print(infD.read());
           Serial3.print(", ");
           Serial3.println(infI.read());
          if((millis()-tiempoini)<2000){robot.antihorario();}
          else{robot.horario();}
          }
          
        }  
    }
    //si detecte pared
    else{
      Serial3.println("Detecte una pared");
      //freno 2 seg 
      robot.detener();
      delay(2000);
      //Salgo de la linea girando hacia la derecha
      while(infD.read() >= 500 || infI.read() >= 500){
          robot.horario();
          }
      //sigo girando hasta que entre a la linea nuevamente
      //(giré 180º)
      while(infD.read() < 500 && infI.read() < 500){
        robot.horario();  
        }
     //configuro la variable para seguir avanzando
     adelante=true;
     volviendo=true;
      } 
 }
  
}
